/*******************************************************************************
*   Copyright (C) Harbin Institute of Technology, Shenzhen.
*				  All Rights Reserved.
*   Department: Decoration
********************************************************************************
* File Name   : sensor.c
* Author      : Dayuan
* Version     : v0.01
* Date        : 2019/9/2
* Description : Decoration Rototics Manipulator task.
*******************************************************************************/
/*-History----------------------------------------------------------------------
* Version   Date      Name        Changes and comments
* v0.01     2019/9/2  Dayuan    initial version
*=============================================================================*/
/* System Includes -----------------------------------------------------------*/

/* Public Includes -----------------------------------------------------------*/
#include "usart.h"
#include "crc.h"
#include "tim.h"
/* Private Includes ----------------------------------------------------------*/
#include "sensor.h"
#include "sensor_if.h"
#include "timer_if.h"

/* Global variables ----------------------------------------------------------*/
SBT_FORCE sbt_force= {0};
/* Private variables ---------------------------------------------------------*/
static SBT_CMD sbt_cmd;
static SBT_COM_STATE sbt_com_state = SBT_COM_IDLE;
/* Global functions ----------------------------------------------------------*/
/**
  * @brief  initial sbt force sensor.
  * @param  none
  * @retval none
  */
void sbt_initial(void)
{
	sbt_com_state = SBT_COM_IDLE;
	sbt_force.over_time_error = 0;
	sbt_force.crc_error = 0;
	sbt_force.detect_valid = 0;
}

/**
  * @brief  sbt force sensor timeout handle.
  * @param  none
  * @retval none
  */
void sbt_timeout_handle(void)
{
	if(sbt_force.over_time_error < 10)
	{
		sbt_force.over_time_error++;
		sbt_com_state = SBT_COM_IDLE;
	}
	else
		return;
}
/**
  * @brief  send baut cmd to sbt sensor.
  * @param  baut rate
  * @retval -1 : wrong baut rate
  */
int sbt_set_buat(uint32_t baut)
{
	uint8_t baut_temp;
	
	sbt_com_state = SBT_COM_WRITE;
	sbt_cmd = SBT_SET_BAUT;
	sbt_force.rx_flag = 0;
	switch(baut)
	{
		case 1200		: baut_temp = BAUT_1200; 	break;
		case 2400		: baut_temp = BAUT_2400; 	break;
		case 4800		: baut_temp = BAUT_4800; 	break;
		case 9600		: baut_temp = BAUT_9600; 	break;
		case 19200	: baut_temp = BAUT_19200;	break;
		case 38400	: baut_temp = BAUT_38400; break;
		case 57600	: baut_temp = BAUT_57600; break;
		case 115200	: baut_temp = BAUT_115200;break;
		case 230400 : baut_temp = BAUT_230400;break;
		default: return -1;
	}
	sbt_force.addr = SBT_485_ADDR;
	sbt_force.fuc  = SBT_485_WRITE;	
	sbt_force.reg_add[0] = SBT_BAUT_ADDR_H;
	sbt_force.reg_add[1] = SBT_BAUT_ADDR_L;		
	sbt_force.reg_num[0] = 0x00;
	sbt_force.reg_num[1] = SBT_BAUT_REGISTER_NUM;
	sbt_force.reg_byte	 = SBT_BAUT_REGISTER_BYTE;
	sbt_send_cf(sbt_cmd, baut_temp);
	
	return 0;
}

/**
  * @brief  set zero cmd to sbt sensor.
  * @param  None.
  * @retval None
  */
void sbt_set_zero(void)
{
	uint8_t data;
	sbt_com_state = SBT_COM_WRITE;
	sbt_cmd = SBT_SET_ZERO;
	sbt_force.rx_flag = 0;
	sbt_force.fuc = SBT_485_WRITE;
	sbt_force.addr = SBT_485_ADDR;
	sbt_force.reg_add[0] = SBT_ZERO_ADDR_H;
	sbt_force.reg_add[1] = SBT_ZERO_ADDR_L;		
	sbt_force.reg_num[0] = 0x00;
	sbt_force.reg_num[1] = SBT_ZERO_REGISTER_NUM;
	sbt_force.reg_byte   = SBT_ZERO_REGISTER_BYTE;
	
	data = 0x01;
	
	sbt_send_cf(sbt_cmd, data);
}

/**
  * @brief  get torque cmd to sbt sensor.
  * @param  None.
  * @retval None
  */
void sbt_get_torque(void)
{
	if(sbt_com_state!= SBT_COM_IDLE){
		sbt_force.com_error++;
		return;
		}
	else{
		sbt_com_state = SBT_COM_WRITE;
		
		sbt_cmd = SBT_GET_TORQUE;
		sbt_force.rx_flag = 0;
		sbt_force.fuc = SBT_485_READ;
		sbt_force.addr = SBT_485_ADDR;
		sbt_force.reg_add[0] = SBT_TORQUE_ADDR_H;
		sbt_force.reg_add[1] = SBT_TORQUE_ADDR_L;		
		sbt_force.reg_num[0] = 0x00;
		sbt_force.reg_num[1] = SBT_TORQUE_REGISTER_NUM;
		sbt_force.reg_byte   = SBT_READ_BYTE;
		
		sbt_send_cf(sbt_cmd, 0);
	}
	if(HAL_TIM_Base_Start_IT(&htim6) != HAL_OK)
	{
		/* Starting Error */
		 Error_Handler();
	}
}

/**
  * @brief  get baut cmd to sbt sensor.
  * @param  None.
  * @retval None
  */
void sbt_get_buat(void)
{
	sbt_com_state = SBT_COM_WRITE;
	sbt_cmd = SBT_GET_BAUT;
	sbt_force.rx_flag = 0;
	sbt_force.fuc = SBT_485_READ;
	sbt_force.addr = SBT_485_ADDR;
	sbt_force.reg_add[0] = SBT_BAUT_ADDR_H;
	sbt_force.reg_add[1] = SBT_BAUT_ADDR_L;		
	sbt_force.reg_num[0] = 0x00;
	sbt_force.reg_num[1] = SBT_BAUT_REGISTER_NUM;	
	sbt_force.reg_byte   = SBT_READ_BYTE;
	
	sbt_send_cf(sbt_cmd,0);
}

/**
  * @brief  get baut cmd to sbt sensor.
  * @param  None.
  * @retval None
  */
void sbt_lock(void)
{
	uint16_t data;
	
	sbt_com_state = SBT_COM_WRITE;
	sbt_cmd = SBT_LOCK;
	sbt_force.rx_flag = 0;
	sbt_force.fuc = SBT_485_WRITE;
	sbt_force.addr = SBT_485_ADDR;
	sbt_force.reg_add[0] = SBT_LOCK_ADDR_H;
	sbt_force.reg_add[1] = SBT_LOCK_ADDR_L;		
	sbt_force.reg_num[0] = 0x00;
	sbt_force.reg_num[1] = SBT_LOCK_REGISTER_NUM;	
	sbt_force.reg_byte 	 = SBT_LOCK_REGISTER_BYTE;
	
	data = SBT_LOCK_CMD;
	
	sbt_send_cf(sbt_cmd,data);
}

/**
  * @brief  get baut cmd to sbt sensor.
  * @param  None.
  * @retval None
  */
void sbt_unlock(void)
{
	uint16_t data;
	
	sbt_com_state = SBT_COM_WRITE;
	sbt_cmd = SBT_UNLOCK;
	sbt_force.rx_flag = 0;
	sbt_force.fuc = SBT_485_WRITE;
	sbt_force.addr = SBT_485_ADDR;
	sbt_force.reg_add[0] = SBT_LOCK_ADDR_H;
	sbt_force.reg_add[1] = SBT_LOCK_ADDR_L;		
	sbt_force.reg_num[0] = 0x00;
	sbt_force.reg_num[1] = SBT_LOCK_REGISTER_NUM;	
	sbt_force.reg_byte	 = SBT_LOCK_REGISTER_BYTE;
	data = SBT_UNLOCK_CMD;
	
	sbt_send_cf(sbt_cmd,data);
}

void sbt_control_polling(void)
{
	static uint16_t sensor_control_cmd = 0;
	static int baut = 115200;
	switch(sensor_control_cmd)
	{
		case 0: return;
		case 1: sbt_unlock(); break;
		case 2: sbt_lock(); break;
		case 3: sbt_set_zero(); break;
		case 4: sbt_set_buat(baut); break;
		case 5: sbt_get_buat();break;
		case 6: sbt_get_torque();break;
		case 7: temp_rd_cf();break;
		case 8: temp_wr_cf();break;
		default:break;
	}
	sensor_control_cmd = 0;
	return;
}

/**
  * @brief  send control field cmd to sbt sensor.
  * @param  W/R cmd and (W)data.
  * @retval -1 : wrong command
  */
int sbt_send_cf(SBT_CMD cmd, uint16_t data)
{
	
	SBT_485_ENABLE_TX();
	
	sbt_force.tx_buf[0] = sbt_force.addr;
	sbt_force.tx_buf[1] = sbt_force.fuc;
	sbt_force.tx_buf[2] = sbt_force.reg_add[0];
	sbt_force.tx_buf[3] = sbt_force.reg_add[1];
	//read command
	if(cmd > SBT_CMD_READ && cmd < SBT_CMD_WRITE){
		HAL_UART_Receive_DMA(&huart2,sbt_force.rx_buf,SBT_485_READ_RX_LEN);

		
		sbt_force.tx_buf[4] = sbt_force.reg_num[0];
		sbt_force.tx_buf[5] = sbt_force.reg_num[1];
		
		sbt_force.txcrc = ModbusCRC_CheckTable(sbt_force.tx_buf, SBT_485_READ_TX_LEN - 2);
		
		sbt_force.tx_buf[6] = sbt_force.txcrc & 0x00ff;
		sbt_force.tx_buf[7] = sbt_force.txcrc >> 8;
		
		sbt_force.tx_buf[8] = 0;
		sbt_force.tx_buf[9] = 0;
		sbt_force.tx_buf[10]= 0;
		
		HAL_UART_Transmit_DMA(&huart2,sbt_force.tx_buf,SBT_485_READ_TX_LEN);
	}
	//write command
	else if(cmd > SBT_CMD_WRITE && cmd <SBT_CMD_ALL){
		HAL_UART_Receive_DMA(&huart2,sbt_force.rx_buf,SBT_485_WRITE_RX_LEN);

		sbt_force.tx_buf[4] = sbt_force.reg_num[0];
		sbt_force.tx_buf[5] = sbt_force.reg_num[1];
		sbt_force.tx_buf[6] = sbt_force.reg_byte;
		sbt_force.tx_buf[7] = data>>8;
		sbt_force.tx_buf[8] = data;
		
		sbt_force.txcrc = ModbusCRC_CheckTable(sbt_force.tx_buf, SBT_485_WRITE_TX_LEN - 2);
		
		sbt_force.tx_buf[9] = sbt_force.txcrc & 0x00ff;
		sbt_force.tx_buf[10]= sbt_force.txcrc >> 8;
		
		HAL_UART_Transmit_DMA(&huart2,sbt_force.tx_buf,SBT_485_WRITE_TX_LEN);
	}
	//wrong command
	else{
		SBT_485_ENABLE_RX();
		return -1;
	}
	return 0;
}

/* Private functions ---------------------------------------------------------*/
/**
  * @brief  get temp data cmd from sbt.
  * @param  None.
  * @retval None
  */
void temp_rd_cf(void)
{
	static uint8_t temp_rd_fuc, temp_rd_addr_l, temp_rd_reg_num;
	
	sbt_cmd = SBT_CUSTOM_READ;
	sbt_force.rx_flag = 0;
	sbt_force.fuc = temp_rd_fuc;
	sbt_force.addr = SBT_485_ADDR;
	sbt_force.reg_add[0] = SBT_BAUT_ADDR_H;
	sbt_force.reg_add[1] = temp_rd_addr_l;		
	sbt_force.reg_num[0] = 0x00;
	sbt_force.reg_num[1] = temp_rd_reg_num;	
	sbt_force.reg_byte   = SBT_READ_BYTE;
	
	sbt_send_cf(sbt_cmd,0);
}

/**
  * @brief  send temp cmd to sbt.
  * @param  None.
  * @retval None
  */
void temp_wr_cf(void)
{
	static uint16_t data;
	static uint8_t temp_wr_fuc, temp_wr_addr_l, temp_wr_reg_num, temp_wr_reg_byte;
	
	sbt_cmd = SBT_CUSTOM_WRITE;
	sbt_force.rx_flag = 0;
	sbt_force.fuc = temp_wr_fuc;
	sbt_force.addr = SBT_485_ADDR;
	sbt_force.reg_add[0] = SBT_LOCK_ADDR_H;
	sbt_force.reg_add[1] = temp_wr_addr_l;		
	sbt_force.reg_num[0] = 0x00;
	sbt_force.reg_num[1] = temp_wr_reg_num;	
	sbt_force.reg_byte 	 = temp_wr_reg_byte;
		
	sbt_send_cf(sbt_cmd,data);
}

/**
  * @brief  UART2 Tx completed callbacks.
  * @param  None.
  * @retval None
  */
void sbt_tx_complete_callback(void)
{
	SBT_485_ENABLE_RX();
	sbt_com_state = SBT_COM_WAIT;
}

/**
  * @brief  UART2 Rx completed callbacks.
  * @param  None.
  * @retval None
  */
uint32_t time_test;
void sbt_rx_complete_callback(void)
{
	int32_t torque_raw;
	static uint32_t time1,time2;
	
	sbt_force.rx_flag = 1;
	sbt_force.detect_valid = 1;
	sbt_com_state = SBT_COM_READ;
	force_timer_reset(); // timer reset
	sbt_force.over_time_error = 0;
	
	time1 = time2;
	time2 = get_sys_time_usec();
	time_test = time2 - time1;
		
	if(sbt_cmd > SBT_CMD_READ && sbt_cmd < SBT_CMD_WRITE)
	{
		sbt_force.rxcrc = ((uint16_t)sbt_force.rx_buf[SBT_485_READ_RX_LEN - 1]<<8) | sbt_force.rx_buf[SBT_485_READ_RX_LEN - 2];
		if(sbt_force.rxcrc != ModbusCRC_CheckTable(sbt_force.rx_buf,SBT_485_READ_RX_LEN - 2))
		{
			sbt_force.crc_error ++;
		}
		else
		{
			torque_raw = ((uint32_t)sbt_force.rx_buf[3]<<24) | ((uint32_t)sbt_force.rx_buf[4]<<16)
												| ((uint32_t)sbt_force.rx_buf[5]<<8) | ((uint32_t)sbt_force.rx_buf[6]);
			sbt_force.torque = ((float)torque_raw) / 1000;
			sbt_force.torque_raw = torque_raw; //test
		}
	}
	else if(sbt_cmd > SBT_CMD_WRITE && sbt_cmd <SBT_CMD_ALL)
	{
		sbt_force.rxcrc = ((uint16_t)sbt_force.rx_buf[SBT_485_WRITE_RX_LEN - 1]<<8) | sbt_force.rx_buf[SBT_485_WRITE_RX_LEN - 2];
		if(sbt_force.rxcrc != ModbusCRC_CheckTable(sbt_force.rx_buf,SBT_485_WRITE_RX_LEN - 2))
		{
			sbt_force.crc_error ++;
		}
		else
		{
			/* write register handle TODO*/			
		}
	}
	else
		return;
	sbt_com_state = SBT_COM_IDLE;
	//sbt_get_torque(); // test
}
/****************************** END OF FILE ***********************************/
